#ifndef DEFINITIONS_H
#define DEFINITIONS_H

// Send PID Controllers and Target Data to SmartDashboard
#define DEBUG_MODE true

/*****************************************************************
*                             PORTS                              *
*****************************************************************/

// Devices
#define IO_CHIP_ATTACHED 	false

//Module Definitions
#define ANALOG_MODULE			 1
#define DRIVE_DIGITAL_MODULE 	 1
#define PAYLOAD_DIGITAL_MODULE	 2
#define SOLENOID_MODULE			 1

//Digital Drive Ports (On Module 1)
	//PWMs
#define LEFT_VICTOR_1 			DRIVE_DIGITAL_MODULE, 1
#define LEFT_VICTOR_2 			DRIVE_DIGITAL_MODULE, 2
#define RIGHT_VICTOR_1 			DRIVE_DIGITAL_MODULE, 3
#define RIGHT_VICTOR_2 			DRIVE_DIGITAL_MODULE, 4
#define RAMP_ARM_SERVO			DRIVE_DIGITAL_MODULE, 5
	//Digital I/O
#define LEFT_ENCODER 			DRIVE_DIGITAL_MODULE, 1, DRIVE_DIGITAL_MODULE, 2
#define RIGHT_ENCODER			DRIVE_DIGITAL_MODULE, 3, DRIVE_DIGITAL_MODULE, 4
#define COMPRESSOR_SWITCH		DRIVE_DIGITAL_MODULE, 5	
	//Relays
#define COMPRESSOR_CHANNEL		DRIVE_DIGITAL_MODULE, 1

//Digital Payload Ports (On Module 2)
	//PWMs
#define PHI_VIC					PAYLOAD_DIGITAL_MODULE, 6
#define SPEED_VIC_1				PAYLOAD_DIGITAL_MODULE, 2
#define SPEED_VIC_2				PAYLOAD_DIGITAL_MODULE, 3
#define BELT_MOTOR_SHOOTING		PAYLOAD_DIGITAL_MODULE, 1
#define BELT_MOTOR_PICKUP		PAYLOAD_DIGITAL_MODULE, 4
#define THETA_VIC				PAYLOAD_DIGITAL_MODULE, 5
	//Digital I/O
#define PHI_ENCODER				PAYLOAD_DIGITAL_MODULE, 3, PAYLOAD_DIGITAL_MODULE, 4
#define SPEED_ENCODER			PAYLOAD_DIGITAL_MODULE, 1, PAYLOAD_DIGITAL_MODULE, 2
#define PHI_LIMIT				PAYLOAD_DIGITAL_MODULE, 5
#define THETA_ENCODER 			PAYLOAD_DIGITAL_MODULE, 6, PAYLOAD_DIGITAL_MODULE, 7
#define THETA_LIMIT				PAYLOAD_DIGITAL_MODULE, 8
	//Relays
#define RAMP_ARM				PAYLOAD_DIGITAL_MODULE, 1
//#define THETA_RELAY			PAYLOAD_DIGITAL_MODULE, 2

//Joysticks
#define DRIVE_STICK_1 			1
#define DRIVE_STICK_2 			2
#define PAYLOAD_STICK_1			3
#define PAYLOAD_STICK_2			4

//Analog
#define GYRO 					ANALOG_MODULE, 1

//Solenoids
#define LEFT_SOLENOID_ON	 	SOLENOID_MODULE, 1
#define LEFT_SOLENOID_OFF		SOLENOID_MODULE, 2
#define RIGHT_SOLENOID_ON	 	SOLENOID_MODULE, 3
#define RIGHT_SOLENOID_OFF		SOLENOID_MODULE, 4
#define SOLENOID_ON				SOLENOID_MODULE, 2
#define SOLENOID_OFF			SOLENOID_MODULE, 1

/*****************************************************************
*                            PARAMETERS                          *
*****************************************************************/

// Shooting Setpoints
#define FENDER_PHI		5
#define FENDER_SPEED	15000

// Autonomous Parameters
#define AUTONOMOUS_SHOOT_DELAY		5.0
#define AUTONOMOUS_PHI_SETPOINT		65
#define AUTONOMOUS_SPEED_SETPOINT	40000

//Drive Parameters
#define DRIVE_PID_PARAMS 			0.0005, 0.00015, -0.00007
#define DRIVE_SHIFT_TRESHOLD_UPPER	2500
#define DRIVE_SHIFT_TRESHOLD_LOWER	2200
#define LOW_GEAR 					false
#define HIGH_GEAR 					!LOW_GEAR
#define RAMP_ARM_TIME				0.3
#define RAMP_SERVO_OUT				0
#define RAMP_SERVO_IN				80

// Payload Parameters
	// general constants
#define M_PI 					3.1415926535897
#define INCHES_PER_FOOT 		12.0
#define GRAVITY 				32 * INCHES_PER_FOOT // (per sec.)^2
#define DEGREES_PER_RADIAN 		(180/M_PI)
#define RADIANS_PER_DEGREE 		1/DEGREES_PER_RADIAN
	// field measurements
#define TARGET_COUNT 			4
#define HOOP_OFFSET 			15 //the wall to the middle of the hoop (in)
#define DELTA_HEIGHT 			34 //how far the target is above the hoop (in)
#define TARGET_WIDTH 			24.0 //vision target width(in)
#define TARGET_HEIGHT 			18.0 //vision target height(in)
#define TARGET_TOP_Y 			109
#define TARGET_MID_Y 			72
#define TARGET_BOT_Y 			39
#define TARGET_RHT_X 			31 //different for competition
#define TARGET_MID_X 			0
#define TARGET_LFT_X 			-31
#define TARGET_H_MAX_ERR 		10
	// camera specs
#define TARGET_COLOR_THRESHOLD 		0, 15, 50, 150, 34, 255 //Target Color Thresholds
#define HORIZONTAL_FOV 				46.5	//horizontal field of view TODO: measure these values for both cameras
#define VERTICAL_FOV   				35.75	//vertical field of view
	// camera position
#define CAMERA_HEIGHT 					33
#define CAMERA_PERPENDICULAR_OFFSET 	0 //to be set once we know stuff from the CAD
#define CAMERA_PARALLEL_OFFSET 			0 // "        "          "           "
#define CAMERA_VERTICAL_OFFSET 			0
#define HORIZONTAL_CAMERA_ANGLE_OFFSET 	-10	//degrees right that the camera is offset
#define VERTICAL_CAMERA_ANGLE_OFFSET 	5	//degrees up from horizontal that the camera points
#define SWISHY_ONLY_RIGHT 				M_PI/6
#define SWISHY_ONLY_LEFT 				5*M_PI/6
#define DESIRED_ENTRY_ANGLE 			M_PI/4
	// Belt Control 
#define BELT_PICKUP_SPEED		0.25
#define BELT_SHOOT_SPEED		0.6
#define BELT_PICKUP_TIME		0.1
#define BELT_SHOOT_TIME			0.8
#define BELT_PICKUP_FORWARD		-1
#define BELT_PICKUP_REVERSE		-BELT_PICKUP_FORWARD
#define BELT_SHOOT_FORWARD		-1
#define BELT_SHOOT_REVERSE		-BELT_SHOOT_FORWARD

	// Turret Control
#define TURRET_WHEEL_DIAMETER 		6.5 //inches
#define TURRET_WHEEL_CIRCUMFERENCE 	(TURRET_WHEEL_DIAMETER * M_PI)
#define TURRET_WHEEL_TOP_RATE 		26.1 //revs per second
#define THETA_PID	 			-0.001, -0.00005, -0.001
#define PHI_PID 				0.01, 0.001, 0.0003
#define SPEED_PID	 			0.0001, 0.00001, 0.0
#define THETA_STEP				16
#define THETA_MIN				-8000
#define THETA_MAX				8000
#define PHI_STEP				2
#define PHI_MAX					140
#define PHI_ZERO_SPEED			-0.3
#define PHI_ZERO_TIME			1.0
#define SPEED_STEP				180
#define SPEED_MAX				40000
#define PHI_MOTOR_MAX			1.0
#define THETA_PULSES_PER_DEGREE	33
#define PHI_PULSES_PER_DEGREE	47.0/16.0
#define PHI_MAX_ANGLE			85
	// Payload Control
#define CONFIDENCE_THRESHOLD_DEFAULT	0.5

#endif
